/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;
// Authors: All Programming Members

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Main extends IterativeRobot {

    private static final boolean DEBUGGING = false;
    
    //Checks autonomous driving
    private boolean autoDone = false;
    
    //Mechanism classes
    private SimpleDrive driving;
    private RobotAngleOptimizer robotAngleOptimizer;
    private ArmClimber arm;
    private Dumper dumper;
    
    
    //Pneumatics objects
//    private Compressor compressor;
//    
//    //Actuator instances
//    private Actuator shiftDriveGears; //green system
    //private Actuator dumperFrontFlap; //yellow system
    //private Actuator dumperLowGoal; //orange syste,
    
    //Objects
    private Joystick leftDriveJoystick;
    private Joystick rightDriveJoystick;
    private Joystick operatorJoystick;
    private Timer timer;
    private double t0 = 0;
    private DigitalInput lsSanity;

    public void robotInit() {
        
        //Initializing timer
        timer = new Timer();
        timer.start();

        //Initializing joysticks
        leftDriveJoystick = new Joystick(RobotMap.LEFT_DRIVE_JOYSTICK);
        rightDriveJoystick = new Joystick(RobotMap.RIGHT_DRIVE_JOYSTICK);
        operatorJoystick = new Joystick(RobotMap.SECOND_DRIVER_JOYSTICK);

        //Initializing mechanisms
        driving = new SimpleDrive(leftDriveJoystick, rightDriveJoystick, RobotMap.LEFT_DRIVE_MOTOR, RobotMap.RIGHT_DRIVE_MOTOR);
        robotAngleOptimizer = new RobotAngleOptimizer(rightDriveJoystick, RobotMap.PUSHER_DOWNER_MOTOR, RobotMap.PUSHER_DOWNER_LIMIT_SWITCH_TOP, RobotMap.PUSHER_DOWNER_LIMIT_SWITCH_BOTTOM, RobotMap.PUSHER_DOWNER_EXTEND, RobotMap.PUSHER_DOWNER_RETRACT, RobotMap.RAO_SWITCH_LINE);
        arm = new ArmClimber(operatorJoystick, RobotMap.ARM_MOTOR_LEFT, RobotMap.ARM_MOTOR_RIGHT, RobotMap.ARM_LIMIT_SWITCH_TOPR, RobotMap.ARM_LIMIT_SWITCH_BOTTOMR, RobotMap.ARM_LIMIT_SWITCH_TOPL, RobotMap.ARM_LIMIT_SWITCH_BOTTOML, RobotMap.RIGHT_ARM_UP, RobotMap.RIGHT_ARM_DOWN, RobotMap.LEFT_ARM_UP, RobotMap.LEFT_ARM_DOWN, RobotMap.BOTH_ARMS_UP, RobotMap.BOTH_ARMS_DOWN, RobotMap.LIMIT_SWITCH_LINE_TOPLEFT, RobotMap.LIMIT_SWITCH_LINE_TOPRIGHT, RobotMap.LIMIT_SWITCH_LINE_BOTTOMLEFT, RobotMap.LIMIT_SWITCH_LINE_BOTTOMRIGHT, RobotMap.BOTH_ARM_UP_SLOW);
        dumper = new Dumper(leftDriveJoystick, RobotMap.DUMPER_MOTOR, RobotMap.DUMPER_UP_BUTTON, RobotMap.DUMPER_DOWN_BUTTON);
        
        //Initializing pneumatics
//        compressor = new Compressor(RobotMap.PRESSURE_SWITCH_CHANNEL, RobotMap.COMPRESSOR_RELAY_CHANNEL);
//        compressor.start();
//        shiftDriveGears = Actuator.buildForSolenoid(RobotMap.SOLENOID_MODULE, RobotMap.DRIVE_CHANNEL, leftDriveJoystick, RobotMap.DRIVE_JOYSTICK_SHIFTER);
//        
//        lsSanity = new DigitalInput(RobotMap.LIMIT_SWITCH_SANITY);
    }

    public void autonomousInit() {
    }

    public void autonomousPeriodic() { //NEEDS TO BE TESTED
        //autoDone = driving.auto(); 3.8.13
        //dumperLowGoal.autonomous(autoDone); // dumps when done driving to the side 3.8.13.
    }

    public void teleopInit() {
    }

    public void teleopPeriodic() {
        //printTime("starting tank drive");
        driving.tankDrive(); //Drive code
        //shiftDriveGears.switchState();
        dumper.dumperControl();
        
        //printTime("start dumper");
        //dumperFrontFlap.switchState(); //Collect frisbees actuator 3.7.13
        //dumperLowGoal.switchState(); //Low dumper actuator 3.7.13
        
        //printTime("start tilt");
        //robotAngleOptimizer.tilt(); //Enables RAO
        
        //printTime("start arm drive");
        arm.armDrive(); //Enables hooks
        robotAngleOptimizer.tilt();
        //DriverStationLCD.getInstance().println(RobotMap.LIMIT_SWITCH_SANITY_LINE, 1, "LS t: " + lsSanity.get());
        //DriverStationLCD.getInstance().updateLCD();
    }

    //Timer to check length of each process
    private void printTime(String title) {
        double tNew = timer.get();
        System.out.println(title + ": " + (tNew - t0));
        t0 = tNew;
    }
}